#include "detection_headers.h"

using namespace NEWMAT;
using namespace DATMO;
using namespace pcl;

//Class definition
class Detection {
public:
        Detection();
        void Distance_Image_Callback(const sensor_msgs::Image::ConstPtr& msg);


        ros::NodeHandle n;
        string trial_name;
protected:

        RobotState RobotStateData;
        StaticDetectionData SDData;
        DynamicDetectionData DDData;

};

//Class Initialization
Detection::Detection(){
        RobotStateData.b_isMovement = false;
        RobotStateData.s_Initial = 0;
        SDData.iteration = 0;

}

//Each time a distance image is received
void Detection::Distance_Image_Callback(const sensor_msgs::Image::ConstPtr& msg)
{
        //Receive distance matrix and convert it to a matrix
        SDData.iteration++;
        ROS_INFO("Iteracion %d", SDData.iteration);
        float* array = (float*)&(msg->data[0]);
        DistanceImage D;
        D.tf_Distance = Matrix((int)msg->height,(int)msg->width);
        int i = 1, j= 1;
        for(int k=0;k<(int)(msg->width * msg->height);k++)
        {
                D.tf_Distance(i,j) = (double)array[k];
                j++;
                if (j > (int)msg->width)
                {
                        i++;j=1;
                }
        }

        //Save as DistanceImage struct
        D.s_height = (int)msg->height;
        D.s_width = (int)msg->width;
        D.CollisionPoints.header = msg->header;
        tf::StampedTransform transform;
        RobotStateData.tf_listener.waitForTransform(msg->header.frame_id, "/world",  msg->header.stamp, ros::Duration(2.0));
        RobotStateData.tf_listener.lookupTransform(msg->header.frame_id, "/world", msg->header.stamp, transform);
        D.CollisionPoints = cl_CollisionMatrix::CollisionMatrix(D,transform);
        //Actualize images
        SDData.LastLastDistImage = SDData.LastDistImage;
        SDData.LastDistImage = SDData.DistImage;
        SDData.DistImage = D;
        if (SDData.iteration == 1)
        {
          SDData.BackgroundRef.tf_Dref = Matrix(D.s_height,D.s_width);
          SDData.BackgroundRef.tf_Dref = str_DistImage.tf_Distance;
          SDData.BackgroundRef.tf_Vref = Matrix(D.s_height,D.s_width);
          SDData.BackgroundRef.tf_Vref = 100000;


        }
        if (SDData.iteration>4)
        {
                //Two different options: robot still and robot moving
                if(RobotStateData.b_isMovement == false) //Robot is not moving
                {
                        if(!cl_CompareLaser::CompareLaser(str_DistImage,str_LastDistImage))
                        {
                                //Detect Objects

                                DetectedObjects str_DetectedObjects = cl_ObjectDetection::ObjectDetection(str_DistImage,str_LastDistImage);
                                cl_PrintFunctions::printMatrix(str_DistImage.tf_Distance,"ACTMAT",iteration, trial_name);
                                cl_PrintFunctions::printMatrix2(str_DistImage.tf_Distance,"ACTMATSEC",iteration, trial_name);
                                cl_PrintFunctions::printMatrix(str_LastDistImage.tf_Distance,"LASTMAT",iteration, trial_name);
                                cl_PrintFunctions::printMatrix(str_DetectedObjects.m_Objects,"OBJ",iteration, trial_name);
                                //Update Reference
                                ROS_INFO("Detection complete");
                                SDData.BackgroundRef = cl_UpdateReference::UpdateReference(SDData.BackgroundRef,SDData.DistImage,SDData.LastDistImage,SDData.LastLastDistImage,str_DetectedObjects.m_Objects);

                                cl_PrintFunctions::printMatrix(str_BackgroundRef.tf_Dref,"DREF",iteration, trial_name);
                                cl_PrintFunctions::printMatrix(str_BackgroundRef.tf_Vref,"aVREF",iteration, trial_name);
                                //Objects2Real
                                if (str_DetectedObjects.i_NumberOfObjects > 0)
                                {
                                        ROS_INFO("Entra en paso a objetos reales");
                                        ros::Time actTime = ros::Time::now();
                                        cl_DynamicObjects2 DynamicObjects_act = cl_Objects2Real::Objects2Real(str_DistImage, str_DetectedObjects, str_BackgroundRef.tf_Dref,
                                                                                                         actTime, str_RobotState.RobotID);
                                        cl_PrintFunctions::printDynObjList(DynamicObjects_act,"OBJREAL",iteration, trial_name);
//                                        ROS_INFO("Entra en identificacion");
//                                        str_RobotState.DynObjects = cl_ObjectIdentification::ObjectIdentification(DynamicObjects_act,
//                                                                                                str_RobotState.DynObjects,actTime,str_RobotState.RobotID);
                                }

                        }
                        else
                        {
                          Matrix auxMat(SDData.BackgroundRef.tf_Dref.Nrows(),SDData.BackgroundRef.tf_Dref.Ncols());
                          auxMat = 0.0;
                          SDData.BackgroundRef = cl_UpdateReference::UpdateReference(SDData.BackgroundRef,SDData.DistImage,SDData.LastDistImage,SDData.LastLastDistImage,auxMat);
                          cl_PrintFunctions::printMatrix(str_BackgroundRef.tf_Dref,"DREF",iteration, trial_name);
                          cl_PrintFunctions::printMatrix(str_BackgroundRef.tf_Vref,"aVREF",iteration, trial_name);
                        }

                }
                else //Robot is moving
                {
                  //convert point cloud to max height map cell
                //      cl_isMovement::isMovement();
                  if () //Center of current map is the same as the previous one
                  {

                  }
                  else  //centers are not the same
                  {

                  }




                }
        }
        else
        {
          SDData.iteration++;

        }
}



int main(int argc, char **argv)
{
        ros::init(argc, argv, "datmo");
        //ROS_INFO("1");

        //ROS_INFO("2");
        Detection *controller = new Detection();

        //ROS Subscribers
        //ROS_INFO("ej");
        //Create time string for test
        time_t now = time(NULL);
        struct tm* now_time = gmtime(&now);
        std::stringstream time_str;
        time_str << now_time->tm_year+1900 <<"_" << now_time->tm_mon+1 <<"_";
        time_str << now_time->tm_mday <<"_" << now_time->tm_hour <<"_";
        time_str << now_time->tm_min <<"_" << now_time->tm_sec <<"_";
        controller->trial_name = time_str.str();
        ros::Subscriber distance_image_sub = controller->n.subscribe("camera/depth/image", 1000, &Detection::Distance_Image_Callback, controller);

        //ROS Publishers
        ros::spin();
        delete controller;

        return 0;
}
